#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
servo_test.py

在 macOS 上通过 ElectronBot 的 Python 低级接口测试舵机控制，
动态检测并测试所有可用的 Bulk IN/OUT 接口 altsetting。
"""
import usb.core, usb.util
import numpy as np
import time
from electron_low_level.electron_low_level import ElectronLowLevel

# Debug: 列出当前所有 USB 设备，检查是否能找到 ElectronBot
print("[Debug] 扫描所有 USB 设备：")
for d in usb.core.find(find_all=True):
    bus = getattr(d, 'bus', None)
    addr = getattr(d, 'address', None)
    print(f"  VID:PID = {hex(d.idVendor)}:{hex(d.idProduct)}  Bus={bus} Addr={addr}")
print("-" * 40)

def find_bulk_interfaces(dev):
    """扫描所有接口的 altsetting，返回 Bulk IN/OUT 候选列表"""
    cfg = dev.get_active_configuration()
    candidates = []
    print("[Info] 可用接口 altsetting 及其 Bulk 端点：")
    for intf in cfg:
        in_eps = [ep for ep in intf
                  if usb.util.endpoint_direction(ep.bEndpointAddress)==usb.util.ENDPOINT_IN
                  and usb.util.endpoint_type(ep.bmAttributes)==usb.util.ENDPOINT_TYPE_BULK]
        out_eps = [ep for ep in intf
                   if usb.util.endpoint_direction(ep.bEndpointAddress)==usb.util.ENDPOINT_OUT
                   and usb.util.endpoint_type(ep.bmAttributes)==usb.util.ENDPOINT_TYPE_BULK]
        print(f"  接口 {intf.bInterfaceNumber} altsetting {intf.bAlternateSetting}: IN={ [hex(ep.bEndpointAddress) for ep in in_eps] } OUT={ [hex(ep.bEndpointAddress) for ep in out_eps] }")
        if in_eps and out_eps:
            candidates.append((intf.bInterfaceNumber,
                               intf.bAlternateSetting,
                               in_eps[0].bEndpointAddress,
                               out_eps[0].bEndpointAddress))
    return candidates


def test_interface(dev, robot, ifno, alt, ep_in, ep_out):
    """在指定接口 altsetting 下测试一次舵机移动"""
    print(f"\n[Test] 接口={ifno}, altsetting={alt}, IN={hex(ep_in)}, OUT={hex(ep_out)}")
    # 切换 altsetting
    try:
        dev.set_interface_altsetting(interface=ifno, alternate_setting=alt)
        print(f"[Debug] 切换到接口 {ifno} altsetting {alt}")
    except Exception as e:
        print(f"[Warning] 切换 altsetting 失败: {e}")
    # 卸载内核驱动并 Claim 接口
    try:
        if dev.is_kernel_driver_active(ifno):
            dev.detach_kernel_driver(ifno)
    except:
        pass
    usb.util.claim_interface(dev, ifno)
    # 更新 SDK 端点
    robot.EP1_IN = ep_in
    robot.EP1_OUT = ep_out
    # 等待 MCU 初始化
    time.sleep(1)
    # 发送空白图像，触发 extraData 拉取
    # frame = np.zeros((240,240,3), dtype=np.uint8)
    # robot.set_image_src(frame)
    robot.sync()
    robot._sync_thread.join()
    time.sleep(0.1)
    # 下发舵机角度命令
    angles_cmd = [30.0, 0.0, 0.0, 0.0, 0.0, 20.0]
    print(f"[Info] 设置舵机角度: {angles_cmd}")
    robot.set_joint_angles(*angles_cmd, True)
    robot.sync()
    robot._sync_thread.join()
    time.sleep(0.5)
    # 读取并打印当前角度
    angles = robot.get_joint_angles()
    print(f"[Info] 接口 {ifno} 返回角度: {angles}")
    input("请观察舵机动作，按回车进入下一个测试...")
    # Release
    try:
        usb.util.release_interface(dev, ifno)
    except:
        pass


def main():
    # [Info] 接口 1 返回角度: [np.float32(13.8218155), np.float32(5.498182), np.float32(-4.0109377), np.float32(2.6181793), np.float32(-21.777773), np.float32(-0.058181763)]
    # [Info] 接口 1 返回角度: [np.float32(13.952728), np.float32(5.4327273), np.float32(-4.0109377), np.float32(2.749092), np.float32(-21.292929), np.float32(-30.836365)]
    #[Info] 接口 1 返回角度: [np.float32(13.8218155), np.float32(5.629091), np.float32(-4.6015034), np.float32(2.6836395), np.float32(-21.874752), np.float32(46.429092)]
    # 初始化并连接
    robot = ElectronLowLevel()
    # 调试连接
    print("[Debug] 尝试连接机器人...")
    try:
        ok = robot.connect()
        print(f"[Debug] connect 返回: {ok}")
    except Exception as e:
        print(f"[Error] connect 异常: {e}")
        return
    if not ok:
        print("[Error] 机器人连接失败: 未找到设备或权限不足，请检查物理连接并尝试 sudo")
        return
    print("[Info] 设备已连接，尝试切换到接口0的 altsetting=1 ...")
    dev = robot.device
    try:
        # 强制切换到 altsetting=1 （用于打开 Bulk 端点）
        dev.set_interface_altsetting(interface=0, alternate_setting=1)
        print("[Debug] 切换到接口0 altsetting=1 成功")
        # 打印该 altsetting 下的端点
        intf1 = dev.get_active_configuration()[(0,1)]
        eps = [f"0x{ep.bEndpointAddress:02X}(attr=0x{ep.bmAttributes:02X})" for ep in intf1]
        print(f"[Debug] 接口0 altsetting1 的端点: {eps}")
    except Exception as e:
        print(f"[Warning] 切换 altsetting1 或获取端点列表失败: {e}")
    print("[Info] 设备已连接，等待 MCU 初始化...")
    time.sleep(2)

    dev = robot.device
    # 枚举并测试所有接口
    candidates = find_bulk_interfaces(dev)
    if not candidates:
        print("[Error] 未找到 Bulk IN/OUT 接口 altsetting，请检查设备"); robot.disconnect(); return
    for ifno, alt, ep_in, ep_out in candidates:
        test_interface(dev, robot, ifno, alt, ep_in, ep_out)

    robot.disconnect()
    print("[Info] 测试完成，已断开设备")


if __name__ == '__main__':
    main() 